
#ifndef _MODEEOPD
#define _MODEEOPD
#endif

#define BSIZE 20

typedef struct
{
  int array[BSIZE];
} BoundArr;

int modeFilter(BoundArr ba, byte numsamples) {
  byte i;
  byte j;
  byte maxoccur_idx =0;
  int sum = 0;
  BoundArr counter;
  for (i=0; i < numsamples; i++) {
    counter.array[i] = 0;
    sum += ba.array[i];
    for (j = 0; j < numsamples; j++) {
      if (ba.array [j] == ba.array[i]){
        counter.array[i]++;
      }
    }
    if (counter.array[i] > counter.array[maxoccur_idx] ) { maxoccur_idx = i; }
  }
  if ( counter.array[maxoccur_idx] == 1){
    return sum / numsamples;
  }
  else{
    return ba.array[maxoccur_idx];
  }
}




int modeEOPD(byte readnum, bool side) {
  int x;
  int reading;
  BoundArr samples;
  for ( x = 0; x < readnum; x++){
  reading = ((side == RIGHT) ? EOPDRReading () : EOPDLReading ());
      samples.array[x] = reading;
      //writeDebugStreamLine("" + reading);
  }
  int _returningMode = modeFilter(samples, readnum);
  return _returningMode;
}

/* int modeEOPDOLD(int readnum, bool side){
  byte howmany[MAXIMUMDIST];
  int x;
  int reading;
  if (readnum < 1){
    return -1;
  }
  for (x = 0; x < readnum; x++){
  reading = HTEOPDreadRaw(((side == RIGHT) ? EOPD_R: EOPD_R));
    if (reading < MAXIMUMDIST){
      howmany[reading] ++;
      writeDebugStreamLine("" + reading);
    }
    else{
      howmany [MAXIMUMDIST - 1]++;
    }
    if (reading > 900){
      return -1;
    }
  }
  int largestindex = 0;
  for (x = 0; x < MAXIMUMDIST; x++){
    if (howmany[x] > howmany[largestindex]) {
      largestindex = x;
    }
  }
  return largestindex;
}
*/
